#ifndef JOYBUTTON_DETECTOR_H
#define JOYBUTTON_DETECTOR_H


#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <iostream>
#include <vector>

#include "conf.h"
// 保存按键状态的全局变量

typedef void (*button_callback_func)(void);

class JoyButtonDetector {
private:
    ros::NodeHandle nh_;
    ros::Subscriber joy_sub_;

public:
    JoyButtonDetector(
        button_callback_func button_0_callback,
        button_callback_func button_1_callback,
        button_callback_func button_3_callback,
        button_callback_func button_4_callback,
        button_callback_func button_6_callback,
        button_callback_func button_7_callback,
        button_callback_func button_10_callback,
        button_callback_func button_11_callback,
        button_callback_func button_13_callback,
        button_callback_func button_14_callback):

        button_0_callback(button_0_callback),
        button_1_callback(button_1_callback),
        button_3_callback(button_3_callback),
        button_4_callback(button_4_callback),
        button_6_callback(button_6_callback),
        button_7_callback(button_7_callback),
        button_10_callback(button_10_callback),
        button_11_callback(button_11_callback),
        button_13_callback(button_13_callback),
        button_14_callback(button_14_callback) {

        // 订阅joy话题
        joy_sub_ = nh_.subscribe("/joy_node_bl/joy", 10, &JoyButtonDetector::joyCallback, this);
        ROS_INFO("Joy按键检测器已启动，等待joy消息...");
    }

    void initializeButtonStates(size_t button_count) {
        if (!initialized) {
            button_pressed_status.resize(button_count, false);
            prev_button_states.resize(button_count, false);
            initialized = true;
            ROS_INFO("检测到joy控制器，按键数量: %zu", button_count);
        }
    }

    void joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg) {
        // 确保状态数组已初始化
        if (!initialized) {
            initializeButtonStates(joy_msg->buttons.size());
        }

        // 检查每个按键的状态
        for (size_t i = 0; i < joy_msg->buttons.size(); ++i) {
            bool current_state = joy_msg->buttons[i] == 1;
            
            // 检测按键是否从按下状态变为松开状态（完整的按键操作）
            if (prev_button_states[i] && !current_state) {
                button_pressed_status[i] = true;  // 保存按键被完整按下的状态

                ROS_INFO("按键 %zu 完成一次按下操作，状态设为true", i);
            }
            
            // 按键刚被按下
            if (!prev_button_states[i] && current_state) {
                ROS_INFO("按键 %zu 被按下", i);
            }
            
            // 更新按键先前状态
            prev_button_states[i] = current_state;
        }

        if (getButtonStatus(0)) {

            sys_log("get button status 0 true: %p", button_0_callback);

            if (button_0_callback) button_0_callback();

            resetButtonStatus(0);
        }

        if (getButtonStatus(1)) {
            sys_log("get button status 1 true: %p", button_1_callback);

            if (button_1_callback) button_1_callback();

            resetButtonStatus(1);
        }

        if (getButtonStatus(3)) {
            sys_log("get button status 3 true: %p", button_3_callback);

            if (button_3_callback) button_3_callback();

            resetButtonStatus(3);
        }

        if (getButtonStatus(4)) {
            sys_log("get button status 4 true: %p", button_4_callback);

            if (button_4_callback) button_4_callback();

            resetButtonStatus(4);
        }

        if (getButtonStatus(6)) {
            sys_log("get button status 6 true: %p", button_6_callback);

            if (button_6_callback) button_6_callback();

            resetButtonStatus(6);
        }

        if (getButtonStatus(7)) {
            sys_log("get button status 7 true: %p", button_7_callback);

            if (button_7_callback) button_7_callback();

            resetButtonStatus(7);
        }

        if (getButtonStatus(10)) {
            sys_log("get button status 10 true: %p", button_10_callback);

            if (button_10_callback) button_10_callback();

            resetButtonStatus(10);
        }

        if (getButtonStatus(11)) {
            sys_log("get button status 11 true: %p", button_11_callback);

            if (button_11_callback) button_11_callback();

            resetButtonStatus(11);
        }

    }

    // 获取指定按键的状态
    bool getButtonStatus(size_t button_index) {
        if (initialized && button_index < button_pressed_status.size()) {
            return button_pressed_status[button_index];
        }
        return false;
    }

    // 重置指定按键的状态
    void resetButtonStatus(size_t button_index) {
        if (initialized && button_index < button_pressed_status.size()){
            button_pressed_status[button_index] = false;
        }
    }

private: 
    std::vector<bool> button_pressed_status;
    std::vector<bool> prev_button_states;
    bool initialized = false;

    button_callback_func button_0_callback; 

    button_callback_func button_1_callback; 

    button_callback_func button_3_callback;

    button_callback_func button_4_callback;

    button_callback_func button_6_callback;

    button_callback_func button_7_callback;

    button_callback_func button_10_callback;

    button_callback_func button_11_callback;

    button_callback_func button_13_callback;

    button_callback_func button_14_callback;
};

#endif // JOYBUTTON_DETECTOR_H
